#pragma once

#include <geometry_msgs/Pose.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>

#include <std_msgs/Float32.h>

namespace Sentry_BT
{
class TransformUtils
{
public:
    TransformUtils(ros::NodeHandle& nh);
    ~TransformUtils() = default;
    
    void publishStaticTransform(const std_msgs::Float32::ConstPtr& msg);
    
    bool transformPoseToBaseLink(const geometry_msgs::Pose& input_pose, 
                                geometry_msgs::Pose& output_pose);
    
private:
    tf2_ros::StaticTransformBroadcaster static_broadcaster_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    ros::Subscriber gimbal_yaw_sub_;
};
}  // namespace Sentry_BT